#include "Config.hpp"
#include <iostream>
#include <boost/thread.hpp>

#include "Estimator.hpp"
#include "Frame.hpp"
#include "PostProcessor.hpp"
#include "common_include.hpp"

std::string cwd = "../../";

typedef visionhw::Config cfg;


int main(int argc, char **argv)
{
    if (argc == 2)
    {
        cwd = std::string(argv[1]);
    }

    visionhw::Config::setConfigFile(cwd + "config.json");
    std::cout << cfg::getValue("intrinsics")["fx"].asDouble() << "\n";

    Json::Value fileDir = cfg::getValue("fileDir");
    // 生成点云
    visionhw::Frame frame_src;
    frame_src.setInputFile(cwd + fileDir["jpegDir"].asString() + fileDir["filename"].asString() + ".jpg",
                           cwd + fileDir["depthDir"].asString() + fileDir["filename"].asString() + ".png");

    frame_src.setCameraConfig(cfg::getValue("intrinsics"));
    frame_src.calcCloud();

    if (cfg::getValue("saveIntermediatePly").asInt())
        pcl::io::savePLYFile("outputply/原始源点云.ply", *frame_src.getCloud());

    visionhw::Frame frame_tgt;
    frame_tgt.setInputFile(cwd + fileDir["plyfilename"].asString());

    // 前处理
    Json::Value post = cfg::getValue("postProcess");

    std::cout << "前处理" << "\n";
    std::cout << "source:" << "\n";
    visionhw::PostProcessor post_src;
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_src(new pcl::PointCloud<pcl::FPFHSignature33>);
    post_src.setInputCloud(frame_src.getCloud());
    post_src.setIsPly(frame_src.is_ply());
    post_src.setSaveIntermediatePly(cfg::getValue("saveIntermediatePly").asInt());
    post_src.setVoxelLeafSize(post["leafSize"]["src"].asDouble());
    post_src.setPlaneFilterPara(post["planeIterations"]["src"].asInt(),
                                post["planeThreshold"]["src"].asDouble());
    post_src.setClusterPara(post["clusterTolerance"]["src"].asDouble(),
                            post["clusterMinSize"]["src"].asInt(),
                            post["clusterMaxSize"]["src"].asInt());
    post_src.setNormalPara(post["normalRadius"]["src"].asDouble());
    post_src.setFPFHPara(post["fpfgRadius"]["src"].asDouble());
    post_src.process(*frame_src.getCloud(), *fpfh_src);

    std::cout << "target:" << "\n";
    visionhw::PostProcessor post_tgt;
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_tgt(new pcl::PointCloud<pcl::FPFHSignature33>);
    post_tgt.setInputCloud(frame_tgt.getCloud());
    post_tgt.setIsPly(frame_tgt.is_ply());
    post_src.setSaveIntermediatePly(false);
    post_tgt.setVoxelLeafSize(post["leafSize"]["tgt"].asDouble());
    post_tgt.setNormalPara(post["normalRadius"]["tgt"].asDouble());
    post_tgt.setFPFHPara(post["fpfgRadius"]["tgt"].asDouble());
    post_tgt.process(*frame_tgt.getCloud(), *fpfh_tgt);

    if (cfg::getValue("saveIntermediatePly").asInt())
        pcl::io::savePLYFile("outputply/标准点云滤波后.ply", *frame_tgt.getCloud());

    // 配准
    std::cout << "配准" << "\n";
    visionhw::Estimator estimator;
    estimator.setSaveIntermediatePly(cfg::getValue("saveIntermediatePly").asInt());

    estimator.setInputSource(frame_src.getCloud(), fpfh_src);
    estimator.setInputTarget(frame_tgt.getCloud(), fpfh_tgt);
    estimator.estimate(*frame_src.getCloud());

    // 可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);

    // 配准后 green
    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_1(frame_src.getCloud(), 0, 255, 0);
    viewer->addPointCloud<PointType>(frame_src.getCloud(), color_1, "src_after_icp");

    // target white
    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_2(frame_tgt.getCloud(), 255, 255, 255);
    viewer->addPointCloud<PointType>(frame_tgt.getCloud(), color_2, "tar");

    if (cfg::getValue("debug")["displayCoordinates"].asInt() == 1)
        viewer->addCoordinateSystem(1);
    viewer->initCameraParameters();

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}